#include "../../head.h"
#include "../include/image.h"
#include "../../DistinguishMode/Armor/include/ArmorDetector.h"
#include "../../DistinguishMode/Rune/include/Rune.h"
#include "../../Common/AngleSolver/include/AngleSolver.h"
#include "../../Common/Prediction/include/PreDiction.h"

using namespace std;
using namespace cv;

//=============== 调参数据 ===============
//#define PROCESS_SHOW //展示过程图片
#define BUFFER_SIZE 1 //设置读取图片缓冲区
#define CAMERA_EXPOSURE -9 //摄像头曝光
#define CAMERA_WIDTH 640//摄像头分辨率宽度
#define CAMERA_HEIGHT 480//摄像头分辨率高度
int ColorMode = 1;//设置我方颜色(1为红，2为蓝)
int HitMode = 4;//设置打击模式,(1为装甲板模式,2为前哨站模式,3为小符,4为大符)
int BULLET_SPEED = 25;//枪口弹速(单位m,需大于10m/s)
//========================================


//=============== 固定参数 ===============
int last_HitMode = 0;//上一次的射击模式
double time_difference = 0.0f;//两帧时间差
volatile unsigned int prdIdx = 0;//设置给ImageData输入图片锁
volatile unsigned int csmIdx = 0;//设置从ImageData读取图片锁
//========================================


struct ImageData {
    Mat img;//图片
    unsigned int frame;//图片序号(帧)
};
ImageData data[BUFFER_SIZE];

void Image::CameraInput()
{
    //======================= 调用摄像头 =========================
    //VideoCapture capture(CAMERA_MODE);//调用摄像头
    VideoCapture capture("C://Users//hp//Desktop//Vision_test//Rune//1.mp4");//读取视频(测试使用)
    capture.set(CV_CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH);//宽度
    capture.set(CV_CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT);//高度
    capture.set(CV_CAP_PROP_EXPOSURE, CAMERA_EXPOSURE);//调节曝光
    if (!capture.isOpened()) {
        cout << "Failed to open camera with index: " << CAMERA_MODE << endl;
    }
    Mat img;//定义局部变量图片
    static bool is_timing = false;
    double last = 0.0f;//上一次输入图片的时间
    double now = 0.0f;//这一次输入图片的时间
    //======================= 图片存进data =========================
    while (1) {
        while (prdIdx - csmIdx >= BUFFER_SIZE);//设置函数锁，防止没读取data图片就被替换
        capture >> img;
        //img = imread("C://Users//hp//Desktop//Vision_test//12.png");//读取图片(测试使用)
        //记录两帧的时间差
        if (is_timing){
            now = static_cast<double>(getTickCount());
            time_difference = (now - last) / getTickFrequency();
            last = static_cast<double>(getTickCount());
        }
        else{
            // 获取开始执行时间
            last = static_cast<double>(getTickCount());
            is_timing = true;
        }
        //将图片存进data
        ::data[prdIdx % BUFFER_SIZE].img = img;
        ::data[prdIdx % BUFFER_SIZE].frame++;
        ++prdIdx;
    }
}

void Image::ImageOutput()
{
    //======================= 局部变量 =========================
    Mat src, src_out;//定义获取输入图片和最终绘制显示图片
    bool Process_mode = false;//展示过程图标志位
    int frame_num = 0;//帧数
    int find_aim = 0;//连续找到目标帧数
    int miss_aim = 0;//连续失去目标帧数
    double last_distance = 0.0f;//上一次的目标距离
    cv::RotatedRect last_armor;//上一次识别到的装甲板旋转矩形
    cv::Point2f last_armor_center = { 0.0f, 0.0f };//上一次的装甲板中心点
    cv::Point2f last_predicted_angle = { 0.0f, 0.0f };//上一次的云台转角

    //======================= 初始化类 =========================
    ArmorDetector Armor_Detector;//装甲板识别类
    Rune Rune_Detector;//神符识别类
    AngleSolver Angle_Solver;//角度解算类
    PreDiction Hit_PreDiction;//打击预测类
    VisionData Vision;//设置串口通信视觉发送电控数据结构体
    ReceiveDate receviceVision;//设置串口通信视觉接收电控数据结构体

    /***********************下面开始循环对每帧图片进行处理**************************/

    while (1) {
        //======================= 读取图片 =========================
        while (prdIdx - csmIdx == 0);//设置函数锁，防止读取没更新的data图片
        ::data[csmIdx % BUFFER_SIZE].img.copyTo(src);
        src.copyTo(src_out);
        frame_num = ::data[csmIdx % BUFFER_SIZE].frame;
        ++csmIdx;
        if (src.empty()) {
            continue;
        }

        //======================= 重置各类数据 =========================
        //切换一次打击模式,就得重置一次数据
        if (last_HitMode != HitMode) {
            Rune_Detector.clear_Data();
            Hit_PreDiction.clear_Data();
            frame_num = 0;
            find_aim = 0;
            miss_aim = 0;
            last_distance = 0.0f;
            last_armor = cv::RotatedRect();
            last_armor_center = { 0.0f, 0.0f };
            last_predicted_angle = { 0.0f, 0.0f };
        }
        last_HitMode = HitMode;

/*********************************装甲板击打/前哨站击打************************************/
        if (HitMode == 1 || HitMode == 2) 
        {
            //======================= 图像处理 =========================
#ifdef PROCESS_SHOW
            Process_mode = true;//如果开启宏定义，把过程图标志位置真
#endif 
            cv::RotatedRect Armor;//定义最终装甲板旋转矩形
            Armor_Detector.SetImage(src, last_armor, miss_aim);//设置处理ROI区域
            Armor_Detector.IamgeThreshold(ColorMode);//二值化
            Armor_Detector.IamgeFindArmor(Armor, Process_mode);//获取装甲板
#ifdef PROCESS_SHOW
            Armor_Detector.IamgeProcessShow();//输出过程图片
#endif 
        //======================= 目标解算 =========================
            bool SovlerResult;
            double distance = 0.0f;//目标距离
            cv::Point2f predicted_angle = { 0.0f, 0.0f };//云台转角
            SovlerResult = Angle_Solver.Get_Solver(Armor.center, predicted_angle);//求解云台转角
            if (SovlerResult) {
                //Angle_Solver.Get_distance_Monocular(Armor, distance, HitMode);//求解距离(单目测距方案)
                Angle_Solver.Get_armor_distance_PNP(Armor, distance);//求解距离(PNP方案)
                Angle_Solver.Get_GUN_Offset(distance, predicted_angle);//进行枪管与相机的y轴偏移补偿
            }

            //======================= 打击预测 =========================
            cv::Point2f predicted_center = { 0.0f, 0.0f };//预测中心点
            if (last_armor_center.x != 0 && last_armor_center.y != 0) {
                //Hit_PreDiction.CompensateGravity1(distance, predicted_angle, BULLET_SPEED);//重力补偿方案一
                Hit_PreDiction.CompensateGravity2(distance, predicted_angle, BULLET_SPEED);//重力补偿方案二
                Hit_PreDiction.Get_FlightTime(distance, BULLET_SPEED);//计算子弹飞行时间
                Hit_PreDiction.Get_Distance_X(last_armor_center, Armor.center);//计算两帧之差装甲板水平位移

                if (HitMode == 1) //装甲板模式水平预测
                {
                    //Hit_PreDiction.CompensateLevel(src.cols, time_difference, Armor.center, predicted_center);//更新预测点
                    //SovlerResult = Angle_Solver.Get_Solver(predicted_center, predicted_angle);//更新yaw轴转角
                }
                else if (HitMode == 2) //前哨站模式预测
                {
                    int Outpost_Direction = 0; //前哨站旋转方向
                    Hit_PreDiction.Get_Outpost_Direction(last_armor_center, Armor, Outpost_Direction);//计算前哨站旋转方向
                    //还没预测出旋转方向,使云台不动
                    if (Outpost_Direction == 0) {
                        predicted_angle = { 0.0f, 0.0f };
                    }
                    else{
                        Hit_PreDiction.Outpost_Compensate(Armor, predicted_center);//前哨站预测
                    }
                }
            }
            last_armor_center = Armor.center;//这次的装甲板中心给到上次的装甲板中心

            //======================= 串口通信 =========================
            //如果SovlerResult为真,则找到目标,否则为失去目标
            if (SovlerResult) {
                Vision = { (float)predicted_angle.x, (float)predicted_angle.y, (float)distance, 0, 1, 0 };//给视觉数据结构体赋值
                last_predicted_angle = predicted_angle;
                last_distance = distance;
                last_armor = Armor;
                find_aim++;
                miss_aim = 0;
            }
            else {
                //由于装甲板被击打中后会熄灭,所以在十帧以内还是丢失目标状态才会停止发送数据,否则判断为被击中
                if (miss_aim > 10) {
                    Vision = { (float)predicted_angle.x, (float)predicted_angle.y, (float)distance, 0, 0, 0 };//给视觉数据结构体赋值
                    last_predicted_angle = { 0.0f, 0.0f };
                    last_distance = 0.0f;
                }
                else {
                    Vision = { (float)last_predicted_angle.x / 3, (float)last_predicted_angle.y / 3 , (float)last_distance, 0, 1, 0 };//给视觉数据结构体赋值
                }
                last_armor_center = { 0.0f, 0.0f };
                miss_aim++;
                find_aim = 0;
            }

            //======================= 展示图片 =========================
            circle(src_out, Angle_Solver.Get_CenterPoint(), 5, Scalar(0, 255, 255), -1, 8);//图像中心
            if (Armor.size.width != 0) {
                Point2f vertex[4];
                Armor.points(vertex);
                for (int i = 0; i < 4; i++) {
                    line(src_out, vertex[i], vertex[(i + 1) % 4], Scalar(0, 255, 255), 2, 8);//装甲板轮廓
                }
                circle(src_out, Armor.center, 3, Scalar(255, 255, 0), -1, 8);//装甲板中心点
                if (predicted_center.x != 0 && predicted_center.y != 0) {
                    circle(src_out, predicted_center, 10, Scalar(0, 255, 255), 2, 8);//预测装甲板中心点
                }
            }
            char str[50];
            sprintf(str, "Armor  %.2f  %.2f Angle %.2f  %.2f Distance %.2f", Armor.center.x, Armor.center.y, Vision.yaw_angle.f, Vision.pitch_angle.f, Vision.dis.f);
            Scalar color_text = ColorMode == 1 ? Scalar(255, 200, 0) : Scalar(0, 66, 255);
            putText(src_out, str, Point(20, 40), CV_FONT_NORMAL, 0.5, color_text, 2, 8);
            imshow("result", src_out);
        }
/*********************************小符击打/大符击打************************************/
        else if (HitMode == 3 || HitMode == 4) {
            //======================= 图像处理 =========================
#ifdef PROCESS_SHOW
            Process_mode = true;//如果开启宏定义，把过程图标志位置真
#endif 
            int Direction = 0;//定义神符旋转方向
            double Rune_R = 0.0f;//定义神符半径
            cv::Point2f RuneCenter = { 0.0f,0.0f };//定义神符中心点
            cv::RotatedRect Rune;//定义最终神符装甲板旋转矩形
            Rune_Detector.SetImage(src);//设置处理ROI区域
            Rune_Detector.IamgeThreshold(ColorMode);//二值化
            Rune_Detector.IamgeFindRune(Rune, RuneCenter, Rune_R, Direction, Process_mode);//获取神符装甲板旋转矩形,神符中心点,神符半径,神符转向
#ifdef PROCESS_SHOW
            Rune_Detector.IamgeProcessShow();//输出过程图片
#endif 
            //======================= 神符解算 =========================
            bool SovlerResult;
            double distance = 0.0f;//目标距离
            cv::Point2f predicted_angle = { 0.0f, 0.0f };//云台转角
            cv::Point2f predicted_center = { 0.0f, 0.0f };//预测中心点
            //Angle_Solver.Get_distance_Monocular(Rune, distance, HitMode);//求解距离(单目测距方案)
            //Angle_Solver.Get_rune_distance_PNP(Rune, RuneCenter, distance);//求解距离(PNP方案)
            Angle_Solver.Get_rune_distance(distance);//求解距离(固定距离方案)

            Hit_PreDiction.Get_FlightTime(distance, BULLET_SPEED);//计算子弹飞行时间
            if (HitMode == 3) //小符预测
            {
                Hit_PreDiction.SmallRune_Compensate(Rune.center, predicted_center, Direction, RuneCenter);//小符预测
            }
            else if (HitMode == 4) //大符预测
            {
                bool jump_rune = Rune_Detector.Get_Jump_Rune();//是否跳符
                Hit_PreDiction.Get_Big_RuneDate(RuneCenter, Rune.center, time_difference, jump_rune);//获取准备大符预测数据
                Hit_PreDiction.BigRune_Compensate(Rune.center, RuneCenter, predicted_center, Rune_R);//打符预测
            }
            SovlerResult = Angle_Solver.Get_Solver(predicted_center, predicted_angle);//求解云台转角
            //Hit_PreDiction.CompensateGravity1(distance, predicted_angle, BULLET_SPEED);//重力补偿方案一
            Hit_PreDiction.CompensateGravity2(distance, predicted_angle, BULLET_SPEED);//重力补偿方案二
            Angle_Solver.Get_GUN_Offset(distance, predicted_angle);//进行枪管与相机的y轴偏移补偿

            //======================= 串口通信 =========================
            //如果SovlerResult为真,则找到目标,否则为失去目标
            if (SovlerResult) {
                Vision = { (float)predicted_angle.x, (float)predicted_angle.y, (float)distance, 0, 1, 0 };//给视觉数据结构体赋值
                last_distance = distance;
            }
            else {
                Vision = { 0.0f, 0.0f, (float)last_distance, 0, 1, 0 };//给视觉数据结构体赋值
            }
            //======================= 展示图片 =========================
            if (Rune.size.width != 0) {
                Point2f vertex[4];
                Rune.points(vertex);
                for (int i = 0; i < 4; i++) {
                    line(src_out, vertex[i], vertex[(i + 1) % 4], Scalar(0, 255, 255), 2, 8);//神符装甲板轮廓
                }
                circle(src_out, Rune.center, 5, Scalar(255, 255, 0), -1, 8);//神符装甲板中心点
                circle(src_out, predicted_center, 10, Scalar(0, 255, 255), 2, 8);//预测装甲板中心点
            }
            if (RuneCenter.x != 0.0f && Rune_R != 0.0f) {
                circle(src_out, RuneCenter, 10, Scalar(255, 255, 0), 2, 8);//神符中心
                circle(src_out, RuneCenter, Rune_R, Scalar(255, 255, 255), 2, 8);//神符轨迹
            }
            Scalar color_text = ColorMode == 1 ? Scalar(0, 66, 255) : Scalar(255, 200, 0);
            putText(src_out, "direction:"+to_string(Direction), Point(20, 40), CV_FONT_NORMAL, 1, color_text, 2, 8);
            char str[50];
            sprintf(str, "Armor  %.2f  %.2f Angle %.2f  %.2f Distance %.2f", Rune.center.x, Rune.center.y, Vision.yaw_angle.f, Vision.pitch_angle.f, Vision.dis.f);
            putText(src_out, str, Point(20, 140), CV_FONT_NORMAL, 0.5, color_text, 2, 8);
            imshow("result", src_out);
        }
        char key = waitKey(1);
        if (key == 'q' || key == 'Q') {
            exit(0);
        }
    }
}